// This is an advanced implementation of the algorithm described in the following paper:
//   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
//     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.

// Modifier: Tong Qin               qintonguav@gmail.com
// 	         Shaozu Cao 		    saozu.cao@connect.ust.hk

// Copyright 2013, Ji Zhang, Carnegie Mellon University
// Further contributions copyright (c) 2016, Southwest Research Institute
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
// 3. Neither the name of the copyright holder nor the names of its
//    contributors may be used to endorse or promote products derived from this
//    software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#include "aloam_velodyne/common.h"
#include "aloam_velodyne/tic_toc.h"
#include <cmath>
#include <nav_msgs/msg/odometry.hpp>
#include <opencv2/opencv.hpp>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <string>
#include <vector>
// #include <ros/ros.h>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.h>
#include <sensor_msgs/msg/point_cloud2.h>
#include <tf2/transform_datatypes.h>
#include <tf2_ros/transform_broadcaster.h>

#include "pclDeskewing.hpp"
#include <sensor_msgs/msg/imu.hpp>

using std::atan2;
using std::cos;
using std::sin;

const double scanPeriod = 0.1;

const int systemDelay = 0;
int systemInitCount = 0;
bool systemInited = false;
int N_SCANS = 0;
float cloudCurvature[400000];
int cloudSortInd[400000];
int cloudNeighborPicked[400000];
int cloudLabel[400000];

bool comp(int i, int j)
{
	return (cloudCurvature[i] < cloudCurvature[j]);
}

// ros::Publisher pubLaserCloud;
// ros::Publisher pubCornerPointsSharp;
// ros::Publisher pubCornerPointsLessSharp;
// ros::Publisher pubSurfPointsFlat;
// ros::Publisher pubSurfPointsLessFlat;
// ros::Publisher pubRemovePoints;
// std::vector<ros::Publisher> pubEachScan;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloud, pubCornerPointsSharp,
	pubCornerPointsLessSharp, pubSurfPointsFlat, pubSurfPointsLessFlat, pubRemovePoints;
std::vector<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr> pubEachScan;

rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr subImu;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubCloudDeskew;
ImuPreintegration imu_inte(100);

bool PUB_EACH_LINE = false;

double MINIMUM_RANGE = 0.1;

template <typename PointT>
void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
							pcl::PointCloud<PointT> &cloud_out, float thres)
{
	if (&cloud_in != &cloud_out)
	{
		cloud_out.header = cloud_in.header;
		cloud_out.points.resize(cloud_in.points.size());
	}

	size_t j = 0;

	for (size_t i = 0; i < cloud_in.points.size(); ++i)
	{
		if (cloud_in.points[i].x * cloud_in.points[i].x +
				cloud_in.points[i].y * cloud_in.points[i].y +
				cloud_in.points[i].z * cloud_in.points[i].z <
			thres * thres)
			continue;
		cloud_out.points[j] = cloud_in.points[i];
		j++;
	}
	if (j != cloud_in.points.size())
	{
		cloud_out.points.resize(j);
	}

	cloud_out.height = 1;
	cloud_out.width = static_cast<uint32_t>(j);
	cloud_out.is_dense = true;
}

std::shared_ptr<rclcpp::Node> node;

std::deque<sensor_msgs::msg::Imu> imuBuf;
std::timed_mutex imuLock;
// receive imu data for de-skewing
void imuHandler(sensor_msgs::msg::Imu imu)
{
	if (imuLock.try_lock_for(std::chrono::milliseconds(10)))
	{
		imuBuf.push_back(imu);
		imuLock.unlock();
	}
}

std::timed_mutex pclLock;
std::deque<sensor_msgs::msg::PointCloud2> pclBuf;

// void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
void laserCloudHandler(sensor_msgs::msg::PointCloud2 laserCloudMsg)
{
	if (pclLock.try_lock_for(std::chrono::milliseconds(10)))
	{
		pclBuf.push_back(laserCloudMsg);
		pclLock.unlock();
	}
}

void process(sensor_msgs::msg::PointCloud2 &laserCloudMsg,
			 std::deque<sensor_msgs::msg::Imu> &imuMsgBuf)
{
	if (!systemInited)
	{
		systemInitCount++;
		if (systemInitCount >= systemDelay)
		{
			systemInited = true;
		}
		else
			return;
	}

	TicToc t_whole;
	TicToc t_prepare;
	std::vector<int> scanStartInd(N_SCANS, 0);
	std::vector<int> scanEndInd(N_SCANS, 0);

	pcl::PointCloud<PointIn> laserCloudDeskew;
	pcl::fromROSMsg(laserCloudMsg, laserCloudDeskew);
	if (laserCloudDeskew.size() < 1)
	{
		std::cerr << "deskewed point cloud empty, this should not happen" << std::endl;
		return;
	}
	// imu pre-integration
	TicToc deskew_tic;

	if (!imu_inte.preintegrate(imuMsgBuf))
	{
		std::cout << std::fixed << std::setprecision(16)
				  << "failed to preintegrate imu data, buffer size " << imuMsgBuf.size()
				  << std::endl;
	}
    else
    {
    	// transform point cloud to start
    	imu_inte.transformPointCloud(laserCloudDeskew);
    	std::cout << "imu deskewing " << laserCloudDeskew.size() << " points with "
    			  << imuMsgBuf.size() << " frames, took " << deskew_tic.toc() << "ms" << std::endl;
    	printf("laser cloud frame time from %lf to %lf\n", laserCloudDeskew.front().timestamp,
    		   laserCloudDeskew.back().timestamp);
    }
	sensor_msgs::msg::PointCloud2 cloud_deskew_msg;
	pcl::toROSMsg(laserCloudDeskew, cloud_deskew_msg);
	cloud_deskew_msg.header = laserCloudMsg.header;
	pubCloudDeskew->publish(cloud_deskew_msg);

	pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
	// pcl::fromROSMsg(laserCloudMsg, laserCloudIn);
	laserCloudIn.points.resize(laserCloudDeskew.points.size());
	int i = 0;
	for (auto p : laserCloudDeskew.points)
	{
		laserCloudIn.points[i].x = p.x;
		laserCloudIn.points[i].y = p.y;
		laserCloudIn.points[i].z = p.z;
		i++;
	}
	std::vector<int> indices;

	pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
	removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);

	int cloudSize = laserCloudIn.points.size();
	float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
	float endOri =
		-atan2(laserCloudIn.points[cloudSize - 1].y, laserCloudIn.points[cloudSize - 1].x) +
		2 * M_PI;

	if (endOri - startOri > 3 * M_PI)
	{
		endOri -= 2 * M_PI;
	}
	else if (endOri - startOri < M_PI)
	{
		endOri += 2 * M_PI;
	}
	// printf("end Ori %f\n", endOri);

	bool halfPassed = false;
	int count = cloudSize;
	PointType point;
	std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
	for (int i = 0; i < cloudSize; i++)
	{
		point.x = laserCloudIn.points[i].x;
		point.y = laserCloudIn.points[i].y;
		point.z = laserCloudIn.points[i].z;

		float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
		int scanID = 0;

		if (N_SCANS == 16)
		{
			scanID = int((angle + 15) / 2 + 0.5);
			if (scanID > (N_SCANS - 1) || scanID < 0)
			{
				count--;
				continue;
			}
		}
		else if (N_SCANS == 32)
		{
			scanID = int((angle + 92.0 / 3.0) * 3.0 / 4.0);
			if (scanID > (N_SCANS - 1) || scanID < 0)
			{
				count--;
				continue;
			}
		}
		else if (N_SCANS == 64)
		{
			if (angle >= -8.83)
				scanID = int((2 - angle) * 3.0 + 0.5);
			else
				scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);

			// use [0 50]  > 50 remove outlies
			if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
			{
				count--;
				continue;
			}
		}
		else
		{
			// printf("wrong scan number\n");
			std::cout << "Wrong scan number" << std::endl;
			// ROS_BREAK();
			rclcpp::shutdown();
		}
		// printf("angle %f scanID %d \n", angle, scanID);
		//  std::cout << "angle " << angle << " scanID " << scanID << std::endl;

		float ori = -atan2(point.y, point.x);
		if (!halfPassed)
		{
			if (ori < startOri - M_PI / 2)
			{
				ori += 2 * M_PI;
			}
			else if (ori > startOri + M_PI * 3 / 2)
			{
				ori -= 2 * M_PI;
			}

			if (ori - startOri > M_PI)
			{
				halfPassed = true;
			}
		}
		else
		{
			ori += 2 * M_PI;
			if (ori < endOri - M_PI * 3 / 2)
			{
				ori += 2 * M_PI;
			}
			else if (ori > endOri + M_PI / 2)
			{
				ori -= 2 * M_PI;
			}
		}

		float relTime = (ori - startOri) / (endOri - startOri);
		point.intensity = scanID + scanPeriod * relTime;
		laserCloudScans[scanID].push_back(point);
	}

	cloudSize = count;
	// printf("points size %d \n", cloudSize);
	std::cout << "points size " << cloudSize << std::endl;

	pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
	for (int i = 0; i < N_SCANS; i++)
	{
		scanStartInd[i] = laserCloud->size() + 5;
		*laserCloud += laserCloudScans[i];
		scanEndInd[i] = laserCloud->size() - 6;
	}

	// printf("prepare time %f \n", t_prepare.toc());
	std::cout << "prepare time " << t_prepare.toc() << std::endl;

	for (int i = 5; i < cloudSize - 5; i++)
	{
		float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x +
					  laserCloud->points[i - 3].x + laserCloud->points[i - 2].x +
					  laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x +
					  laserCloud->points[i + 1].x + laserCloud->points[i + 2].x +
					  laserCloud->points[i + 3].x + laserCloud->points[i + 4].x +
					  laserCloud->points[i + 5].x;
		float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y +
					  laserCloud->points[i - 3].y + laserCloud->points[i - 2].y +
					  laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y +
					  laserCloud->points[i + 1].y + laserCloud->points[i + 2].y +
					  laserCloud->points[i + 3].y + laserCloud->points[i + 4].y +
					  laserCloud->points[i + 5].y;
		float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z +
					  laserCloud->points[i - 3].z + laserCloud->points[i - 2].z +
					  laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z +
					  laserCloud->points[i + 1].z + laserCloud->points[i + 2].z +
					  laserCloud->points[i + 3].z + laserCloud->points[i + 4].z +
					  laserCloud->points[i + 5].z;

		cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
		cloudSortInd[i] = i;
		cloudNeighborPicked[i] = 0;
		cloudLabel[i] = 0;
	}

	TicToc t_pts;

	pcl::PointCloud<PointType> cornerPointsSharp;
	pcl::PointCloud<PointType> cornerPointsLessSharp;
	pcl::PointCloud<PointType> surfPointsFlat;
	pcl::PointCloud<PointType> surfPointsLessFlat;

	float t_q_sort = 0;
	for (int i = 0; i < N_SCANS; i++)
	{
		if (scanEndInd[i] - scanStartInd[i] < 6) continue;
		pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
		for (int j = 0; j < 6; j++)
		{
			int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;
			int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;

			TicToc t_tmp;
			std::sort(cloudSortInd + sp, cloudSortInd + ep + 1, comp);
			t_q_sort += t_tmp.toc();

			int largestPickedNum = 0;
			for (int k = ep; k >= sp; k--)
			{
				int ind = cloudSortInd[k];

				if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > 0.1)
				{

					largestPickedNum++;
					if (largestPickedNum <= 2)
					{
						cloudLabel[ind] = 2;
						cornerPointsSharp.push_back(laserCloud->points[ind]);
						cornerPointsLessSharp.push_back(laserCloud->points[ind]);
					}
					else if (largestPickedNum <= 20)
					{
						cloudLabel[ind] = 1;
						cornerPointsLessSharp.push_back(laserCloud->points[ind]);
					}
					else
					{
						break;
					}

					cloudNeighborPicked[ind] = 1;

					for (int l = 1; l <= 5; l++)
					{
						float diffX =
							laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
						float diffY =
							laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
						float diffZ =
							laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
						if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
						{
							break;
						}

						cloudNeighborPicked[ind + l] = 1;
					}
					for (int l = -1; l >= -5; l--)
					{
						float diffX =
							laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
						float diffY =
							laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
						float diffZ =
							laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
						if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
						{
							break;
						}

						cloudNeighborPicked[ind + l] = 1;
					}
				}
			}

			int smallestPickedNum = 0;
			for (int k = sp; k <= ep; k++)
			{
				int ind = cloudSortInd[k];

				if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < 0.1)
				{

					cloudLabel[ind] = -1;
					surfPointsFlat.push_back(laserCloud->points[ind]);

					smallestPickedNum++;
					if (smallestPickedNum >= 4)
					{
						break;
					}

					cloudNeighborPicked[ind] = 1;
					for (int l = 1; l <= 5; l++)
					{
						float diffX =
							laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
						float diffY =
							laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
						float diffZ =
							laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
						if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
						{
							break;
						}

						cloudNeighborPicked[ind + l] = 1;
					}
					for (int l = -1; l >= -5; l--)
					{
						float diffX =
							laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
						float diffY =
							laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
						float diffZ =
							laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
						if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
						{
							break;
						}

						cloudNeighborPicked[ind + l] = 1;
					}
				}
			}

			for (int k = sp; k <= ep; k++)
			{
				if (cloudLabel[k] <= 0)
				{
					surfPointsLessFlatScan->push_back(laserCloud->points[k]);
				}
			}
		}

		pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
		pcl::VoxelGrid<PointType> downSizeFilter;
		downSizeFilter.setInputCloud(surfPointsLessFlatScan);
		downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
		downSizeFilter.filter(surfPointsLessFlatScanDS);

		surfPointsLessFlat += surfPointsLessFlatScanDS;
	}
	// printf("sort q time %f \n", t_q_sort);
	std::cout << "sort q time " << t_q_sort << std::endl;
	// printf("seperate points time %f \n", t_pts.toc());
	std::cout << "seperate points time" << t_pts.toc() << std::endl;

	// sensor_msgs::PointCloud2 laserCloudOutMsg;
	sensor_msgs::msg::PointCloud2 laserCloudOutMsg;
	pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
	// laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
	laserCloudOutMsg.header.stamp = laserCloudMsg.header.stamp;
	// laserCloudOutMsg.header.frame_id = "/camera_init";
	laserCloudOutMsg.header.frame_id = "laser_init";
	// pubLaserCloud.publish(laserCloudOutMsg);
	pubLaserCloud->publish(laserCloudOutMsg);

	// sensor_msgs::PointCloud2 cornerPointsSharpMsg;
	sensor_msgs::msg::PointCloud2 cornerPointsSharpMsg;
	pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
	// cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
	cornerPointsSharpMsg.header.stamp = laserCloudMsg.header.stamp;
	// cornerPointsSharpMsg.header.frame_id = "/camera_init";
	cornerPointsSharpMsg.header.frame_id = "laser_init";
	// pubCornerPointsSharp.publish(cornerPointsSharpMsg);
	pubCornerPointsSharp->publish(cornerPointsSharpMsg);

	// sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
	sensor_msgs::msg::PointCloud2 cornerPointsLessSharpMsg;
	pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
	// cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
	cornerPointsLessSharpMsg.header.stamp = laserCloudMsg.header.stamp;
	// cornerPointsLessSharpMsg.header.frame_id = "/camera_init";
	cornerPointsLessSharpMsg.header.frame_id = "laser_init";
	// pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);
	pubCornerPointsLessSharp->publish(cornerPointsLessSharpMsg);

	// sensor_msgs::PointCloud2 surfPointsFlat2;
	sensor_msgs::msg::PointCloud2 surfPointsFlat2;
	pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
	// surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
	surfPointsFlat2.header.stamp = laserCloudMsg.header.stamp;
	// surfPointsFlat2.header.frame_id = "/camera_init";
	surfPointsFlat2.header.frame_id = "laser_init";
	// pubSurfPointsFlat.publish(surfPointsFlat2);
	pubSurfPointsFlat->publish(surfPointsFlat2);

	// sensor_msgs::PointCloud2 surfPointsLessFlat2;
	sensor_msgs::msg::PointCloud2 surfPointsLessFlat2;
	pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
	// surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
	surfPointsLessFlat2.header.stamp = laserCloudMsg.header.stamp;
	// surfPointsLessFlat2.header.frame_id = "/camera_init";
	surfPointsLessFlat2.header.frame_id = "laser_init";
	// pubSurfPointsLessFlat.publish(surfPointsLessFlat2);
	pubSurfPointsLessFlat->publish(surfPointsLessFlat2);

	// pub each scam
	if (PUB_EACH_LINE)
	{
		for (int i = 0; i < N_SCANS; i++)
		{
			// sensor_msgs::PointCloud2 scanMsg;
			sensor_msgs::msg::PointCloud2 scanMsg;
			pcl::toROSMsg(laserCloudScans[i], scanMsg);
			// scanMsg.header.stamp = laserCloudMsg->header.stamp;
			scanMsg.header.stamp = laserCloudMsg.header.stamp;
			// scanMsg.header.frame_id = "/camera_init";
			scanMsg.header.frame_id = "laser_init";
			// pubEachScan[i].publish(scanMsg);
			pubEachScan[i]->publish(scanMsg);
		}
	}

	printf("scan registration time %f ms *************\n", t_whole.toc());
	if (t_whole.toc() > 100)
		// ROS_WARN("scan registration process over 100ms");
		RCLCPP_WARN(node->get_logger(), "scan registration process over 100ms");
}

void process_thread()
{
	sensor_msgs::msg::PointCloud2 laserCloudMsg;
	std::deque<sensor_msgs::msg::Imu> imuMsgBuf;

	while (rclcpp::ok())
	{
		if (!pclLock.try_lock_for(std::chrono::milliseconds(10)))
		{
			std::this_thread::sleep_for(std::chrono::milliseconds(10));
			continue;
		}
		if (pclBuf.empty())
		{
			pclLock.unlock();
			std::this_thread::sleep_for(std::chrono::milliseconds(10));
			continue;
		}
		laserCloudMsg = pclBuf.front();
		pclBuf.pop_front();
		pclLock.unlock();

		if (!imuLock.try_lock_for(std::chrono::milliseconds(10)))
		{
			std::this_thread::sleep_for(std::chrono::milliseconds(10));
			continue;
		}

		double curtime = stamp_to_sec(laserCloudMsg.header.stamp);
		while (!imuBuf.empty())
		{
			if (stamp_to_sec(imuBuf.front().header.stamp) < curtime - 0.1 || imuBuf.size() > 20)
			{
				imuBuf.pop_front(); // pop old data
			}
			else
				break;
		}
		imuMsgBuf = imuBuf;
		imuLock.unlock();

		if (imuMsgBuf.empty())
		{
			// imu offline
			std::cerr << "imu buffer empty, skip deskewing" << std::endl;
			// continue;
		}

		process(laserCloudMsg, imuMsgBuf);
	}
}

int main(int argc, char **argv)
{
	std::cout << "scan registration node started" << std::endl;
	// ros::init(argc, argv, "scanRegistration");
	rclcpp::init(argc, argv);
	// ros::NodeHandle nh;
	node = std::make_shared<rclcpp::Node>("scanRegistration");

	// nh.param<int>("scan_line", N_SCANS, 16);
	node->declare_parameter("scan_line", 16);
	node->get_parameter("scan_line", N_SCANS);

	// nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);
	node->declare_parameter("minimum_range", 0.1);
	node->get_parameter("minimum_range", MINIMUM_RANGE);

	// printf("scan line number %d \n", N_SCANS);
	std::cout << "scan line number " << N_SCANS << std::endl;

	if (N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)
	{
		// printf("only support velodyne with 16, 32 or 64 scan line!");
		std::cout << "only support velodyne with 16, 32 or 64 scan line!" << std::endl;
		return 0;
	}

	// ros::Subscriber subLaserCloud =
	// nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);
	rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subLaserCloud =
		node->create_subscription<sensor_msgs::msg::PointCloud2>("/velodyne_points", 100,
																 laserCloudHandler);

	// pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100);
	pubLaserCloud =
		node->create_publisher<sensor_msgs::msg::PointCloud2>("/velodyne_cloud_2", 100);

	// pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);
	pubCornerPointsSharp =
		node->create_publisher<sensor_msgs::msg::PointCloud2>("/laser_cloud_sharp", 100);

	// pubCornerPointsLessSharp =
	// nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100);
	pubCornerPointsLessSharp =
		node->create_publisher<sensor_msgs::msg::PointCloud2>("/laser_cloud_less_sharp", 100);

	// pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);
	pubSurfPointsFlat =
		node->create_publisher<sensor_msgs::msg::PointCloud2>("/laser_cloud_flat", 100);

	// pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat",
	// 100);
	pubSurfPointsLessFlat =
		node->create_publisher<sensor_msgs::msg::PointCloud2>("/laser_cloud_less_flat", 100);

	// pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_remove_points", 100);
	pubRemovePoints =
		node->create_publisher<sensor_msgs::msg::PointCloud2>("/laser_remove_points", 100);

	// imu subscription
	subImu = node->create_subscription<sensor_msgs::msg::Imu>("imu_data", 100, imuHandler);

	pubCloudDeskew =
		node->create_publisher<sensor_msgs::msg::PointCloud2>("/laser_cloud_deskew", 100);

	if (PUB_EACH_LINE)
	{
		for (int i = 0; i < N_SCANS; i++)
		{
			// ros::Publisher tmp = nh.advertise<sensor_msgs::PointCloud2>("/laser_scanid_" +
			// std::to_string(i), 100);
			rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr tmp =
				node->create_publisher<sensor_msgs::msg::PointCloud2>(
					"/laser_scanid_" + std::to_string(i), 100);
			pubEachScan.push_back(tmp);
		}
	}

	std::thread processor(process_thread);
	processor.detach();

	// ros::spin();
	rclcpp::spin(node);
	rclcpp::shutdown();

	return 0;
}
